/*============================================================================*/
/*                          The Freescale Cup                                 */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         apptask.c%
* %version:         1 %
* %created_by:      Continental %
* %date_created:     %
*=============================================================================*/
/* DESCRIPTION :  All tasks are called here                                   */
/*============================================================================*/
/* FUNCTION COMMENT : Add here your tasks, inside a periodic task             */
/* according to the desired execution period                                  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 22/08/2011  |  Revision                     | Continental      */
/*                                                                            */
/*============================================================================*/

/* Includes */
/* ------------------- */
/* Common declarations */
/* ------------------- */
#include "apptask.h"

/*       SW API    */
/* --------------- */
#include "APPLeds.h"

/* Exported functions prototypes */
/* ----------------------------- */
void apptask_3ms_LeftMotor(void);
void apptask_3ms_RightMotor(void);
void apptask_5ms(void);
void apptask_10ms(void);
void apptask_20ms(void);
void apptask_20ms_Servo(void);
void apptask_100ms(void);
void apptask_500ms(void);
void apptask_1s(void);
void appcamara_20ms(void);
void delayled (void);


extern int32_t Leftencoder;
extern int32_t Rightencoder;
extern int32_t percent_LeftMotor=0;
extern int32_t percent_RightMotor=0;
int32_t volante;
int j,l,resultado[128],res_menor,menor,center_line;

/**************************************************************
 *  Name                 : 	apptask_3ms_LeftMotor
 *	ID					 : 	TASK_3MS_LeftMotor;
 *  Description          :  Left Motor Controller
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_3ms_LeftMotor(void)
 {
  percent_LeftMotor =Leftencoder*100;
	pwm_LeftMotor(percent_LeftMotor);	
 }
 

/**************************************************************
 *  Name                 : 	apptask_3ms_RightMotor
 *	ID					 : 	TASK_3MS_RightMotor;
 *  Description          :	Right Motor Controller
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_3ms_RightMotor(void)
 {
	percent_RightMotor =Rightencoder*100;
	pwm_rightMotor(percent_RightMotor);
 }
 
/**************************************************************
 *  Name                 : apptask_5ms
 *	ID					 : TASK_5MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_5ms(void)
 {
	
 }
 
 /**************************************************************
 *  Name                 : apptask_10ms
 *	ID					 : TASK_10MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_10ms(void)
 {
	
 }
 
 /**************************************************************
 *  Name                 : apptask_20ms
 *	ID					 : TASK_20MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_20ms(void)
 {
 //	static T_UBYTE lub_20ms_counter = 1;
 	
 	/*Run every 40 ms (20 ms task * 2)*/
 //	if((lub_20ms_counter % 2)==0)
 //	{
 		/*Add the task to execute every 40 ms*/
 //		APPLeds_Led1_app(); 	
 //	}
 //	if((lub_20ms_counter % 16)==0)
 //	{
 //		/*Add the task to execute every 320 ms*/
 //		APPLeds_Led3_app();
 		/*Clear counter but will be set in next instruccion*/
 //		lub_20ms_counter = 0;
 //	}
 //	lub_20ms_counter++;
 }
 
 // funcion del servomotor 
  /**************************************************************
 *  Name                 : servomotor
 *	ID					 : direccion
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_20ms_Servo(void)
 {
 volante=(16*center_line)+1000;
 if(volante>1278) volante=1278;
 if(volante<723) volante=723;
 
 pwm_servo_Motor(volante);
 }
 
 /**************************************************************
 *  Name                 : apptask_100ms
 *	ID					 : TASK_100MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_100ms(void)
 {
/* 
  if(SIU.GPDI[3].B.PDI==1) //Leemos el pin 3, corresponde PA[3]
   {
   	SIU.GPDO[69].B.PDO = 1;//Ponemos a 1 el led 3
   }
   else
   {
   	SIU.GPDO[69].B.PDO = 0;////Ponemos a 1 el led 3
   }
  */
 }
 
 /**************************************************************
 *  Name                 : apptask_500ms
 *	ID					 : TASK_500MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_500ms(void)
 {
  
   	
 }
 
  /**************************************************************
 *  Name                 : apptask_700ms
 *	ID					 : TASK_700MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_700ms(void)
 {
 	
 }
 
 /**************************************************************
 *  Name                 : apptask_1s
 *	ID					 : TASK_1S
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_1s(void)
 {
  extern int resultado[128];
  int a;
  a=1;
 }
 
 /**************************************************************
 *  Name                 : apptask_EXT0_Interrupt
 *	ID					 : TASK_EXT0_Interrupt
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_EXT0_Interrupt(void)
 {
 
 }
 
 /**************************************************************
 *  Name                 : apptask_EXT1_Interrup
 *	ID					 : TASK_EXT1_Interrupt
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_EXT1_Interrupt(void)
 {
 
 }
 
  /**************************************************************
 *  Name                 : appcamara_20ms
 *	ID					 : TASK_CAMARA
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void appcamara_20ms(void)
 {
 
 


/*********************  Initialization Function(s) ************************/

/* ----------------------------------------------------------- */
/*                System Initialization Function                  */
/* ----------------------------------------------------------- */
   
 //  while(1)
 //  {
   	
   
   
         SIU.PGPDO[2].R = 0x08000000;      //SI=PE4
         delayled();
         SIU.PGPDO[2].R |= 0x04000000;      //CLK=PE5
         delayled();
         SIU.PGPDO[2].R &= 0x0700000;
         delayled();
         SIU.PGPDO[2].R &= 0x0B000000;
         delayled();
         for(j=1;j<=128;j++) //for para adquirir los 128 pixeles
         {
            delayled();
            SIU.PGPDO[2].R |= 0x04000000;      //CLK=PE5
            ADC.MCR.B.NSTART=1;                //COMIENZA LA CONVERSION
            while (ADC.MSR.B.NSTART == 1) {};  //ESPERA HASTA TERMINAR LA CONVERSION
            resultado[j]= ADC.CDR[0].B.CDATA;     //ALMACENA RESULTADO DE CONVERSION   
            delayled();
            SIU.PGPDO[2].R &= 0x0B000000; 
            
            
        }
        res_menor=resultado[15];
        menor=15;
        for(l=16;l<=113;l++)
        {
        if(resultado[l]<res_menor)
        {
           res_menor=resultado[l];
           menor=l;
        }
           
        }
        
           	l=0;
            j=0;
            center_line=menor-64;
 //  }
 }

void delayled(void)  // Delay de 0.55 us
{
   int i; 
   
   for(i=0;i<1;i++)
   {   }
}

 
 
  
 